#include "zf_common_headfile.h"

//#define SW_1    P33_5  // 茂驴陆茂驴陆茂驴陆茂驴陆茂驴陆茂驴陆茂驴陆茂驴陆茂驴陆脧掳茂驴陆茂驴陆茂驴陆茂驴陆茂驴陆脫娄茂驴陆茂驴陆茂驴陆茂驴陆
//#define SW_2    P33_4  // 茂驴陆茂驴陆茂驴陆茂驴陆茂驴陆茂驴陆茂驴陆茂驴陆茂驴陆脧掳茂驴陆茂驴陆茂驴陆茂驴陆茂驴陆脫娄茂驴陆茂驴陆茂驴陆茂驴陆

// 摁键状态,及其上个状态
uint8 sw1_status = 0, sw2_status = 0;
uint8 sw1_last_status = 0, sw2_last_status = 0;

void display_init (void)
{
    key_init(10); //10
    gpio_init(SW_1, GPI, 1, GPI_FLOATING_IN);
    gpio_init(SW_2, GPI, 1, GPI_FLOATING_IN);
    gpio_init(SW_3, GPI, 1, GPI_FLOATING_IN);
    gpio_init(SW_4, GPI, 1, GPI_FLOATING_IN);
    gpio_init(KEY_1, GPI, 0, GPI_PULL_DOWN );
    gpio_init(KEY_2, GPI, 0, GPI_PULL_DOWN );
    gpio_init(KEY_3, GPI, 0, GPI_PULL_DOWN );
    gpio_init(KEY_4, GPI, 0, GPI_PULL_DOWN );
    gpio_init(KEY_5, GPI, 0, GPI_PULL_DOWN );

    ips200_init(IPS200_TYPE_PARALLEL8);
}

void show_add_line (void)
{
    // 在逆透视图像上显示的线

    int rheight = RESULT_ROW, rweight = RESULT_COL;
    // 显示等距采样后的左边线
    for (int i = 0; i < rpts0s_num; i++)
    {
        // 描出左边线的点,显示红色
        if (rpts0s[i][0] < rweight && rpts0s[i][1] < rheight && rpts0s[i][0] > 0 && rpts0s[i][1] > 0)
            ips200_draw_point((uint16) rpts0s[i][0], (uint16) rpts0s[i][1], RGB565_RED);
        // 描出左边线点右移一个像素的点
        if (rpts0s[i][0] + 1 < rweight && rpts0s[i][1] < rheight && rpts0s[i][0] + 1 > 0 && rpts0s[i][1] > 0)
            ips200_draw_point((uint16) rpts0s[i][0] + 1, (uint16) rpts0s[i][1], RGB565_RED);
        // 描出左边线点左移一个像素的点
        if (rpts0s[i][0] - 1 < rweight && rpts0s[i][1] < rheight && rpts0s[i][0] - 1 > 0 && rpts0s[i][1] > 0)
            ips200_draw_point((uint16) rpts0s[i][0] - 1, (uint16) rpts0s[i][1], RGB565_RED);
    }
    // 显示等距采样后的右边线,显示蓝色
    for (int i = 0; i < rpts1s_num; i++)
    {
        // 描出右边线的点
        if (rpts1s[i][0] < rweight && rpts1s[i][1] < rheight && rpts1s[i][0] > 0 && rpts1s[i][1] > 0)
            ips200_draw_point((uint16) rpts1s[i][0], (uint16) rpts1s[i][1], RGB565_BLUE);
        // 描出右边线点左移一个像素的点
        if (rpts1s[i][0] - 1 < rweight && rpts1s[i][1] < rheight && rpts1s[i][0] - 1 > 0 && rpts1s[i][1] > 0)
            ips200_draw_point((uint16) rpts1s[i][0] - 1, (uint16) rpts1s[i][1], RGB565_BLUE);
        // 描出右边线点右移一个像素的点
        if (rpts1s[i][0] + 1 < rweight && rpts1s[i][1] < rheight && rpts1s[i][0] + 1 > 0 && rpts1s[i][1] > 0)
            ips200_draw_point((uint16) rpts1s[i][0] + 1, (uint16) rpts1s[i][1], RGB565_BLUE);
    }

    // 显示圆环左线,显示黄色
    for (int i = 0; i < crpts0s_num; i++)
    {
        // 描出左边线的点
        if (crpts0s[i][0] < rweight && crpts0s[i][1] < rheight && crpts0s[i][0] > 0 && crpts0s[i][1] > 0)
            ips200_draw_point((uint16) crpts0s[i][0], (uint16) crpts0s[i][1], RGB565_YELLOW);
        // 描出左边线点左移一个像素的点
        if (crpts0s[i][0] - 1 < rweight && crpts0s[i][1] < rheight && crpts0s[i][0] - 1 >0 && crpts0s[i][1] > 0)
            ips200_draw_point((uint16) crpts0s[i][0] - 1, (uint16) crpts0s[i][1], RGB565_YELLOW);
        // 描出左边线点右移一个像素的点
        if (crpts0s[i][0] + 1 < rweight && crpts0s[i][1] < rheight && crpts0s[i][0] + 1 > 0 && crpts0s[i][1] > 0)
            ips200_draw_point((uint16) crpts0s[i][0] + 1, (uint16) crpts0s[i][1], RGB565_YELLOW);
    }
    // 显示圆环右线,显示青色
    for (int i = 0; i < crpts1s_num; i++)
    {
        // 描出右边线的点
        if (crpts1s[i][0] < rweight && crpts1s[i][1] < rheight && crpts1s[i][0] > 0 && crpts1s[i][1] > 0)
            ips200_draw_point((uint16) crpts1s[i][0], (uint16) crpts1s[i][1], RGB565_CYAN);
        // 描出右边线点左移一个像素的点
        if (crpts1s[i][0] - 1 < rweight && crpts1s[i][1] < rheight && crpts1s[i][0] - 1 > 0 && crpts1s[i][1] > 0)
            ips200_draw_point((uint16) crpts1s[i][0] - 1, (uint16) crpts1s[i][1], RGB565_CYAN);
        // 描出右边线点右移一个像素的点
        if (crpts1s[i][0] + 1 < rweight && crpts1s[i][1] < rheight && crpts1s[i][0] + 1 > 0 && crpts1s[i][1] > 0)
            ips200_draw_point((uint16) crpts1s[i][0] + 1, (uint16) crpts1s[i][1], RGB565_CYAN);
    }

    // 显示十字左远线,显示黄色
    for (int i = 0; i < far_rpts0s_num; i++)
    {
        // 描出左边线的点
        if (far_rpts0s[i][0] < rweight && far_rpts0s[i][1] < rheight && far_rpts0s[i][0] > 0 && far_rpts0s[i][1] > 0)
            ips200_draw_point((uint16) far_rpts0s[i][0], (uint16) far_rpts0s[i][1], RGB565_YELLOW);
        // 描出左边线点右移一个像素的点
        if (far_rpts0s[i][0] + 1 < rweight && far_rpts0s[i][1] < rheight && far_rpts0s[i][0] + 1 > 0 && far_rpts0s[i][1] > 0)
            ips200_draw_point((uint16) far_rpts0s[i][0] + 1, (uint16) far_rpts0s[i][1], RGB565_YELLOW);
        // 描出左边线点左移一个像素的点
        if (far_rpts0s[i][0] - 1 < rweight && far_rpts0s[i][1] < rheight && far_rpts0s[i][0] - 1 > 0 && far_rpts0s[i][1] > 0)
            ips200_draw_point((uint16) far_rpts0s[i][0] - 1, (uint16) far_rpts0s[i][1], RGB565_YELLOW);
    }
    // 显示十字右远线,显示青色
    for (int i = 0; i < far_rpts1s_num; i++)
    {
        // 描出右边线的点
        if (far_rpts1s[i][0] < rweight && far_rpts1s[i][1] < rheight && far_rpts1s[i][0] > 0 && far_rpts1s[i][1] > 0)
            ips200_draw_point((uint16) far_rpts1s[i][0], (uint16) far_rpts1s[i][1], RGB565_CYAN);
        // 描出右边线点左移一个像素的点
        if (far_rpts1s[i][0] - 1 < rweight && far_rpts1s[i][1] < rheight && far_rpts1s[i][0] - 1 >0 && far_rpts1s[i][1] > 0)
            ips200_draw_point((uint16) far_rpts1s[i][0] - 1, (uint16) far_rpts1s[i][1], RGB565_CYAN);
        // 描出右边线点右移一个像素的点
        if (far_rpts1s[i][0] + 1 < rweight && far_rpts1s[i][1] < rheight && far_rpts1s[i][0] + 1 >0 && far_rpts1s[i][1] > 0)
            ips200_draw_point((uint16) far_rpts1s[i][0] + 1, (uint16) far_rpts1s[i][1], RGB565_CYAN);
    }
//    // 显示十字左中线
//    for (int i = 0; i < far_rptsc0_num; i++)
//    {
//        if (far_rptsc0[i][0] < rweight && far_rptsc0[i][1] < rheight)
//            ips200_draw_point((uint16) far_rptsc0[i][0], (uint16) far_rptsc0[i][1], RGB565_RED);
//    }
//    // 显示十字右中线
//    for (int i = 0; i < far_rptsc1_num; i++)
//    {
//        if (far_rptsc1[i][0] < rweight && rptsc1[i][1] < rheight)
//            ips200_draw_point((uint16) far_rptsc1[i][0], (uint16) far_rptsc1[i][1], RGB565_BLUE);
//    }

    // 显示左中线
    for (int i = 0; i < rptsc0_num; i++)
    {
        if (rptsc0[i][0] < rweight && rptsc0[i][1] < rheight)
            ips200_draw_point((uint16) rptsc0[i][0], (uint16) rptsc0[i][1], RGB565_RED);
    }
    // 显示右中线
    for (int i = 0; i < rptsc1_num; i++)
    {
        if (rptsc1[i][0] < rweight && rptsc1[i][1] < rheight)
            ips200_draw_point((uint16) rptsc1[i][0], (uint16) rptsc1[i][1], RGB565_BLUE);
    }

    // 显示预瞄点
    ips200_show_sign_point((uint16) rptsn[aim_idx][0], (uint16) rptsn[aim_idx][1], RGB565_RED);

    // 显示预瞄点所在数组,显示绿色
    for (int i = 0; i < rptsn_num; i++)
    {
        // 描出当前点
        if (rptsn[i][0] < rweight && rptsn[i][1] < rheight && rptsn[i][0] > 0 && rptsn[i][1] > 0)
            ips200_draw_point((uint16) rptsn[i][0], (uint16) rptsn[i][1], RGB565_GREEN);
        // 描出当前点左移一个像素的点
        if (rptsn[i][0] - 1 < rweight && rptsn[i][1] < rheight && rptsn[i][0] - 1 > 0 && rptsn[i][1] > 0)
            ips200_draw_point((uint16) rptsn[i][0] - 1, (uint16) rptsn[i][1], RGB565_GREEN);
        // 描出当前点左移一个像素的点
        if (rptsn[i][0] + 1 < rweight && rptsn[i][1] < rheight && rptsn[i][0] + 1 > 0 && rptsn[i][1] > 0)
            ips200_draw_point((uint16) rptsn[i][0] + 1, (uint16) rptsn[i][1], RGB565_GREEN);
    }

    // 在原始图像上显示的线
    int height = USED_ROW, weight = USED_COL;

    // 显示爬出的原始左边线,显示红色
    for (int i = 0; i < ipts0_num; i++)
    {
        // 描出左边线的点
        if (ipts0[i][0] < weight && ipts0[i][1] < height && ipts0[i][0] > 0 && ipts0[i][1] > 0)
            ips200_draw_point((uint16) ipts0[i][0], (uint16) ipts0[i][1] + 190, RGB565_RED);
        // 描出左边线点左移一个像素的点
        if (ipts0[i][0] - 1 < weight && ipts0[i][1] < height && ipts0[i][0] - 1 > 0 && ipts0[i][1] > 0)
            ips200_draw_point((uint16) ipts0[i][0] - 1, (uint16) ipts0[i][1] + 190, RGB565_RED);
        // 描出左边线点右移一个像素的点
        if (ipts0[i][0] + 1 < weight && ipts0[i][1] < height && ipts0[i][0] + 1 > 0 && ipts0[i][1] > 0)
            ips200_draw_point((uint16) ipts0[i][0] + 1, (uint16) ipts0[i][1] + 190, RGB565_RED);
    }
    // 显示爬出的原始右边线,显示蓝色
    for (int i = 0; i < ipts1_num; i++)
    {
        // 描出右边线的点
        if (ipts1[i][0] < weight && ipts1[i][1] < height && ipts1[i][0] > 0 && ipts1[i][1] > 0)
            ips200_draw_point((uint16) ipts1[i][0], (uint16) ipts1[i][1] + 190, RGB565_BLUE);
        // 描出右边线点左移一个像素的点
        if (ipts1[i][0] - 1 < weight && ipts1[i][1] < height && ipts1[i][0] - 1 > 0 && ipts1[i][1] > 0)
            ips200_draw_point((uint16) ipts1[i][0] - 1, (uint16) ipts1[i][1] + 190, RGB565_BLUE);
        // 描出右边线点右移一个像素的点
        if (ipts1[i][0] + 1 < weight && ipts1[i][1] < height && ipts1[i][0] + 1 > 0 && ipts1[i][1] > 0)
            ips200_draw_point((uint16) ipts1[i][0] + 1, (uint16) ipts1[i][1] + 190, RGB565_BLUE);
    }

    // 显示出圆环左线,显示黄色
    for (int i = 0; i < cipts0_num; i++)
    {
        // 描出左边线的点
        if (cipts0[i][0] < weight && cipts0[i][1] < height && cipts0[i][0] > 0 && cipts0[i][1] > 0)
            ips200_draw_point((uint16) cipts0[i][0], (uint16) cipts0[i][1] + 190, RGB565_YELLOW);
        // 描出左边线点左移一个像素的点
        if (cipts0[i][0] - 1 < weight && cipts0[i][1] < height && cipts0[i][0] - 1 > 0 && cipts0[i][1] > 0)
            ips200_draw_point((uint16) cipts0[i][0] - 1, (uint16) cipts0[i][1] + 190, RGB565_YELLOW);
        // 描出左边线点右移一个像素的点
        if (cipts0[i][0] + 1 < weight && cipts0[i][1] < height && cipts0[i][0] + 1 > 0 && cipts0[i][1] > 0)
            ips200_draw_point((uint16) cipts0[i][0] + 1, (uint16) cipts0[i][1] + 190, RGB565_YELLOW);
    }
    // 显示出圆环右线,显示青色
    for (int i = 0; i < cipts1_num; i++)
    {
        // 描出右边线的点
        if (cipts1[i][0] < weight && cipts1[i][1] < height && cipts1[i][0] > 0 && cipts1[i][1] > 0)
            ips200_draw_point((uint16) cipts1[i][0], (uint16) cipts1[i][1] + 190, RGB565_CYAN);
        // 描出右边线点左移一个像素的点
        if (cipts1[i][0] - 1 < weight && cipts1[i][1] < height && cipts1[i][0] - 1 > 0 && cipts1[i][1] > 0)
            ips200_draw_point((uint16) cipts1[i][0] - 1, (uint16) cipts1[i][1] + 190, RGB565_CYAN);
        // 描出右边线点右移一个像素的点
        if (cipts1[i][0] + 1 < weight && cipts1[i][1] < height && cipts1[i][0] + 1 > 0 && cipts1[i][1] > 0)
            ips200_draw_point((uint16) cipts1[i][0] + 1, (uint16) cipts1[i][1] + 190, RGB565_CYAN);
    }

    // 显示爬出的十字左远线,显示黄色
    for (int i = 0; i < far_ipts0_num; i++)
    {
        // 描出左边线的点
        if (far_ipts0[i][0] < weight && far_ipts0[i][1] < height && far_ipts0[i][0] > 0 && far_ipts0[i][1] > 0)
            ips200_draw_point((uint16) far_ipts0[i][0], (uint16) far_ipts0[i][1] + 190, RGB565_YELLOW);
        // 描出左边线点左移一个像素的点
        if (far_ipts0[i][0] - 1 < weight && far_ipts0[i][1] < height && far_ipts0[i][0] - 1 > 0 && far_ipts0[i][1] > 0)
            ips200_draw_point((uint16) far_ipts0[i][0] - 1, (uint16) far_ipts0[i][1] + 190, RGB565_YELLOW);
        // 描出左边线点右移一个像素的点
        if (far_ipts0[i][0] + 1 < weight && far_ipts0[i][1] < height && far_ipts0[i][0] + 1 > 0 && far_ipts0[i][1] > 0)
            ips200_draw_point((uint16) far_ipts0[i][0] + 1, (uint16) far_ipts0[i][1] + 190, RGB565_YELLOW);
    }
    // 显示爬出的十字右远线,显示青色
    for (int i = 0; i < far_ipts1_num; i++)
    {
        // 描出右边线的点
        if (far_ipts1[i][0] < weight && far_ipts1[i][1] < height && far_ipts1[i][0] > 0 && far_ipts1[i][1] > 0)
            ips200_draw_point((uint16) far_ipts1[i][0], (uint16) far_ipts1[i][1] + 190, RGB565_CYAN);
        // 描出右边线点左移一个像素的点
        if (far_ipts1[i][0] - 1 < weight && far_ipts1[i][1] < height && far_ipts1[i][0] - 1 > 0 && far_ipts1[i][1] > 0)
            ips200_draw_point((uint16) far_ipts1[i][0] - 1, (uint16) far_ipts1[i][1] + 190, RGB565_CYAN);
        // 描出右边线点右移一个像素的点
        if (far_ipts1[i][0] + 1 < weight && far_ipts1[i][1] < height && far_ipts1[i][0] + 1 > 0 && far_ipts1[i][1] > 0)
            ips200_draw_point((uint16) far_ipts1[i][0] + 1, (uint16) far_ipts1[i][1] + 190, RGB565_CYAN);
    }

//    // 显示出车库左线,显示黄色
//    for (int i = 0; i < gipts0_num; i++)
//    {
//        // 描出左边线的点
//        if (gipts0[i][0] < weight && gipts0[i][1] < height && gipts0[i][0] > 0 && gipts0[i][1] > 0)
//            ips200_draw_point((uint16) gipts0[i][0], (uint16) gipts0[i][1] + 190, RGB565_YELLOW);
//        // 描出左边线点左移一个像素的点
//        if (gipts0[i][0] - 1 < weight && gipts0[i][1] < height && gipts0[i][0] - 1 > 0 && gipts0[i][1] > 0)
//            ips200_draw_point((uint16) gipts0[i][0] - 1, (uint16) gipts0[i][1] + 190, RGB565_YELLOW);
//        // 描出左边线点右移一个像素的点
//        if (gipts0[i][0] + 1 < weight && gipts0[i][1] < height && gipts0[i][0] + 1 > 0 && gipts0[i][1] > 0)
//            ips200_draw_point((uint16) gipts0[i][0] + 1, (uint16) gipts0[i][1] + 190, RGB565_YELLOW);
//    }
//    // 显示出车库右线,显示青色
//    for (int i = 0; i < gipts1_num; i++)
//    {
//        // 描出右边线的点
//        if (gipts1[i][0] < weight && gipts1[i][1] < height && gipts1[i][0] > 0 && gipts1[i][1] > 0)
//            ips200_draw_point((uint16) gipts1[i][0], (uint16) gipts1[i][1] + 190, RGB565_CYAN);
//        // 描出右边线点左移一个像素的点
//        if (gipts1[i][0] - 1 < weight && gipts1[i][1] < height && gipts1[i][0] - 1 > 0 && gipts1[i][1] > 0)
//            ips200_draw_point((uint16) gipts1[i][0] - 1, (uint16) gipts1[i][1] + 190, RGB565_CYAN);
//        // 描出右边线点右移一个像素的点
//        if (gipts1[i][0] + 1 < weight && gipts1[i][1] < height && gipts1[i][0] + 1 > 0 && gipts1[i][1] > 0)
//            ips200_draw_point((uint16) gipts1[i][0] + 1, (uint16) gipts1[i][1] + 190, RGB565_CYAN);
//    }
}

void display_entry (void)
{
    ips200_bgcolor = RGB565_WHITE;

    if (!sw2_status)
    {
        // 拨下摁键2,显示菜单
        if (!menu_level)
            menu_display();
        else
            menu2_display();
    }
    else if (!sw1_status)
    {
        // 拨下摁键1,显示原始图像,逆透视图像,以及一些数据
        if (mt9v03x_finish_flag == 0)
        {
            // 显示逆透视图像
            ips200_show_gray_image_vec(0, 0, (uint8* (*)[RESULT_COL])PerImg_ip, RESULT_COL, RESULT_ROW, RESULT_COL, RESULT_ROW, thres);

            // 显示灰度原始图像
            ips200_show_gray_image(0, 190, mt9v03x_image[0], MT9V03X_W, MT9V03X_H, MT9V03X_W, MT9V03X_H, thres);

            // 添加爬出的和处理后的线
            show_add_line();

            // 显示电机控制数据
            ips200_show_float(110, 0, servo_duty, 2, 2);    // 方向偏差
            ips200_show_int(120, 1, motor_l.current_speed, 3);      // 显示左电机当前速度
//            ips200_show_int(110, 2, motor_l.current_speed_raw, 3);  // 显示左电机原始速度
            ips200_show_int(120, 2, motor_r.current_speed, 3);      // 显示右电机当前速度
//            ips200_show_int(110, 4, motor_r.current_speed_raw, 3);  // 显示右电机原始速度
            ips200_show_int(120, 3, motor_l.duty, 4);   // 显示左电机占空比
            ips200_show_int(120, 4, motor_r.duty, 4);   // 显示右电机占空比
//            ips200_show_int(170, 3, motor_l.target_speed, 3);    // 显示左电机目标速度
//            ips200_show_int(170, 4, motor_r.target_speed, 3);    // 显示右电机目标速度

//            // 边线拐角状态
            ips200_show_int(170, 5, is_bend0, 1);
            ips200_show_int(170, 6, is_bend1, 1);

            // 角度
            ips200_show_float(170, 3, pittch, 3, 3);
            ips200_show_float(170, 4, Yaw, 3, 3);

//            // 边线长度
//            ips200_show_int(170, 0, rptsc0_num, 3);
//            ips200_show_int(170, 1, rptsc1_num, 3);
//            ips200_show_int(170, 2, far_rptsc0_num, 3);
//            ips200_show_int(170, 3, far_rptsc1_num, 3);

            ips200_show_int(170, 0, rpts_num, 3);
            ips200_show_int(170, 1, set_speed, 3);
            ips200_show_float(170, 2, aim_distance, 3, 3);

            // 显示状态标志位
            ips200_show_string(0, 6, track_type_name[track_type]);
            ips200_show_string(0, 7, cross_type_name[cross_type]);
            ips200_show_string(0, 8, circle_type_name[circle_type]);
            ips200_show_string(0, 9, ramp_type_name[ramp_type]);
            ips200_show_string(0, 10, stop_car_type_name[stop_car_type]);
        }
    }
}
